import rclpy
from rclpy.node import Node
import numpy as np
import sensor_msgs.msg as sensor_msgs

class PointCloudPublisher(Node):
    def __init__(self):
        super().__init__('point_cloud_publisher')
        self.publisher = self.create_publisher(sensor_msgs.PointCloud2, 'point_cloud_topic', 10)
        self.timer = self.create_timer(1.0, self.publish_pointcloud)

    def publish_pointcloud(self):
        points = np.random.rand(100, 3).astype(np.float32)  # 生成随机点云
        msg = sensor_msgs.PointCloud2()
        msg.header.frame_id = 'map'
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.height = 1
        msg.width = len(points)
        msg.fields = [
            sensor_msgs.PointField(name='x', offset=0, datatype=7, count=1),  # FLOAT32
            sensor_msgs.PointField(name='y', offset=4, datatype=7, count=1),
            sensor_msgs.PointField(name='z', offset=8, datatype=7, count=1)
        ]
        msg.is_bigendian = False
        msg.point_step = 12  # 每个点占12字节 (x, y, z各4字节)
        msg.row_step = msg.point_step * msg.width
        msg.data = points.tobytes()
        msg.is_dense = True
        self.publisher.publish(msg)

def main():
    rclpy.init()
    node = PointCloudPublisher()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
